#include "QP_Intf.h"
#include "Protocol.h"
#include "Transmiter.h"

#include "Logger.h"

std::string g_str_SW_VERSION = "V_0_9_0";

TcpSocket TcpIns;
Proto PtoIns(TcpIns);

// std::shared_ptr<spdlog::logger> log_itf = LogMan::instance();  // 日志器成员变量

int TP_DN_Init(void)
{
    bool ret;

    if(PtoIns.LoadOrigin() != TP_DN_STATUS_SUCCES)  //加载原点坐标
    {   
        return -1;  
    }
    
    // ret = PtoIns.create_and_listen_socket(8080);

    ret = PtoIns.create_connect_to_server(8080);

    return (ret==true)? 0:-1;
}

/**
* 功能:设置系统日志
* 参数：0表示不打印日志，1表示简单打印日志，2表示详细日志。默认值为1
* 返回值：TP_DN_STATUS_SUCCES表示成功，其它表示失败
* 注：需要规定日志的路径及名称，EXE所有路径的父目录下的Log文件夹，
* TP_DN_Log_当前日期时间.txt。
*/
int TP_DN_SetDebugLog(int level)
{
    LogMan::Set_Level(level);

    return 0;
}

/**
* 功能：获取SDK版本信息
* 参数：std::string
* 返回值：TP_DN_STATUS_SUCCES表示成功，其它表示失败
*/
int TP_DN_GetVersion(std::string & info)
{
    info = g_str_SW_VERSION;
}


bool TP_Is_Connected(void)
{
    bool ret;
    ret = PtoIns.Is_Socket_Connected();

    return ret;
}

bool TP_Is_AllDrv_Enable(void)  //是否驱动器已经全部使能
{
    return PtoIns.Is_All_Drv_Enable();
}

int TP_DN_SetPosition(std::vector<tpDNRouteStruct> const & pos_routes, int pos_interval) //设置设备位置（指定位置、路径且让探头移动到该位置）
{
    bool ret;
    ret = PtoIns.SetRoutePts(pos_routes,static_cast<uint32_t>(pos_routes.size()),true);
    return (ret==true)? 0:-1;
}


int TP_DN_SetRoutes(std::vector<tpDNRouteStruct> const & routes, int interval)  
{
    bool ret;
    ret = PtoIns.SetRoutePts(routes,static_cast<uint32_t>(routes.size()),false);
    return (ret==true)? 0:-1;
}

int TP_DN_StartRoutes(int trigger_delay)
{
    bool ret;
    ret = PtoIns.Cmd_StartRun(trigger_delay);
    return (ret==true)? 0:-1;
}

int TP_DN_PauseRoutes(void)
{
    bool ret;
    ret = PtoIns.Cmd_PauseRun();
    return (ret==true)? 0:-1;
}

int TP_DN_ResumeRoutes(void)
{
    bool ret;
    ret = PtoIns.Cmd_ResumeRun();
    return (ret==true)? 0:-1;
}

int TP_DN_StopRoutes(void)
{
    bool ret;
    ret = PtoIns.Cmd_StopRun();
    return (ret==true)? 0:-1;
}

bool TP_DN_DO_Ccontrol(uint8_t id,uint8_t on_off)  //DO 输出控制
{
    bool ret;
    ret = PtoIns.Cmd_DO_Ctrl(id,on_off);
    return (ret==true)? 0:-1;
}

bool TP_DN_Routes_Finish(void)  //是否点位路径运行结束
{
    return PtoIns.Is_Motion_Fin();
}

int TP_DN_GetPosition(tpDNPositionStruct & pos) //获取设备位置信息
{
    bool ret;
    ret = PtoIns.Cmd_Get_Dev_Pos(pos);
    return (ret==true)? 0:-1;
}

int TP_DN_GetCurrentRoute(int & index, tpDNRouteStruct & route)  //获取开始执行路径规划后，当前的路径信息
{
    bool ret;
    ret = PtoIns.Cmd_Get_CurPts_Infos(index, route);
    return (ret==true)? 0:-1;
}

int TP_DN_Refresh_Rt_Infos() //刷新一次设备实时信息
{
    bool ret;
    ret = PtoIns.Cmd_Get_RT_Infos();
    return (ret==true)? 0:-1;
}

int TP_DN_SingleMove(int axis,float distance,float speed)   //单独控制各个轴的移动
{
    bool ret;
    ret = PtoIns.Cmd_Move_Sigle_Axis(axis,distance,speed);
    return (ret==true)? 0:-1;
}

int TP_DN_SetDefaultSpeed(int axis, float v)  //设置默认的速度
{
    bool ret;
    bool succ = false;
    ret = PtoIns.Cmd_Set_Default_Spd(axis,v,&succ);

    if(ret && succ)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

int TP_DN_GetDefaultSpeed(int axis, float & v) //获取电机的默认速度
{
    bool ret;
    bool succ = false;
    ret = PtoIns.Cmd_Get_Default_Spd(axis,&v,&succ);

    if(ret && succ)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

int TP_DN_SetOrigin(int index, tpDNOriginEncoderStruct & encoder) //设置坐标原点
{
    int ret = 0;
    ret = PtoIns.SetOrigin(index, encoder);    //设置原点坐标点，并且存入文件
    if(ret == 0)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

int TP_DN_LoadOrigin(void)    //加载原点坐标
{
    bool ret;

    if(PtoIns.LoadOrigin() != TP_DN_STATUS_SUCCES)  
    {   return -1;  }
    
    return TP_DN_STATUS_SUCCES;
}

int TP_DN_GetOriginEncoder(tpDNOriginEncoderStruct & encoder)  //获取坐标原点的编码器值
{
    int ret = 0;
    ret = PtoIns.GetOriginEncoder(encoder);  //获取原点坐标
    
    if(ret == 0)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}




int TP_DN_BackTo_Origin(tpDNPositionStruct & cur_pos)  //回到原点接口
{
    int ret;

    int Num_Pts = 1000;

    float x_delta;
    float y_delta;
    float z_delta;
    float rx_delta;
    float rz_delta;

    x_delta = (0 - cur_pos.x) / Num_Pts;
    y_delta = (0 - cur_pos.y) / Num_Pts;
    z_delta = (0 - cur_pos.z) / Num_Pts;
    rx_delta = (0 - cur_pos.rx) / Num_Pts;
    rz_delta = (0 - cur_pos.rz) / Num_Pts;

    std::vector<tpDNRouteStruct> pts;

    pts.reserve(Num_Pts);

    for (int i = 0; i < (Num_Pts-1); ++i) {
        // 1. 创建临时对象（每次循环生成新对象）
        tpDNRouteStruct tempRoute{
            .pos = {
            .x = cur_pos.x  + (i) * x_delta,
            .y = cur_pos.y  + (i) * y_delta,
            .z = cur_pos.z  + (i) * z_delta,
            .rx = cur_pos.rx  + (i) * rx_delta,
            .rz = cur_pos.rz  + (i) * rz_delta,
            .ry = 0,
            }
            
        };

        pts.push_back(std::move(tempRoute));
    }

    tpDNRouteStruct tempRoute{
        .pos = {
        .x = 0,
        .y = 0,
        .z = 0,
        .rx = 0,
        .rz = 0,
        .ry = 0, //原点位置
        }
        
    };
    pts.push_back(std::move(tempRoute));


    ret = TP_DN_SetRoutes(pts,10);
    if(ret != 0)
    { return ret;}

    ret = TP_DN_StartRoutes(0);
    if(ret != 0)
    { return ret;}
    
    return 0;
}


//---虚拟点位发送---
int DBG_Virtual_Pts_Set_Forward_WaitFin(tpDNPositionStruct & cur_pos)
{
    int DBG_Num_Pts = 20000; //1000;
    std::vector<tpDNRouteStruct> pts;

    pts.reserve(DBG_Num_Pts);
    
    for (int i = 0; i < DBG_Num_Pts; ++i)
    {
        // 1. 创建临时对象（每次循环生成新对象）
        tpDNRouteStruct tempRoute{
            .pos = {
            .x = cur_pos.x + (i+1) * (0.08/1.0f),   //10s内走完全程，所有每10ms走20
            .y = cur_pos.y + (i+1) * (0.08/1.0f),
            .z = cur_pos.z + (i+1) * (0.08/1.0f),
            .rx = cur_pos.rx + (i+1) * (0.360/1.0f),
            .rz = cur_pos.rz + (i+1) * (0.360/1.0f),
            .ry = cur_pos.ry + 0  // 显式初始化保留字段
            },
            .vx = 200.0f,
            .vy = 200.0f,
            .vz = 200.0f,
            .vrx = 200.0f,
            .vry = 200.0f,  // 显式初始化保留字段
            .vrz = 200.0f,
            .ax = 10.0f,
            .ay = 10.0f,
            .az = 10.0f,
            .arx = 10.0f,
            .ary = 10.0f,   // 显式初始化保留字段
            .arz = 10.0f
            
        };



        pts.push_back(std::move(tempRoute));
    }

    return TP_DN_SetPosition(pts,10);
}

//---虚拟点位发送---
int DBG_Virtual_Pts_Set_Forword(tpDNPositionStruct & cur_pos)  //用到了 TP_DN_SetPosition接口
{
    int DBG_Num_Pts = 1000;
    std::vector<tpDNRouteStruct> pts;

    pts.reserve(DBG_Num_Pts);
    
    for (int i = 0; i < DBG_Num_Pts; ++i) {
        // 1. 创建临时对象（每次循环生成新对象）
        tpDNRouteStruct tempRoute{
            // .pos = {
            // .x = cur_pos.x  + (i+1) * 0.02,
            // .y = cur_pos.y  + (i+1) * 0.02,
            // .z = cur_pos.z  + (i+1) * 0.02,
            // .rx = cur_pos.rx  + (i+1) * 0.36,
            // .rz = cur_pos.rz  + (i+1) * 0.36,
            // .ry = cur_pos.ry  + 0, // 显式初始化保留字段
            // },
            .pos = {
            .x = 0,
            .y = 0,
            .z = 0,
            .rx = 0,
            .rz = 0,
            .ry = 0, // 显式初始化保留字段
            },
            .vx = 200.0f,
            .vy = 200.0f,
            .vz = 200.0f,
            .vrx = 200.0f,
            .vry = 200.0f,  // 显式初始化保留字段
            .vrz = 200.0f,
            .ax = 10.0f,
            .ay = 10.0f,
            .az = 10.0f,
            .arx = 10.0f,
            .ary = 10.0f,   // 显式初始化保留字段
            .arz = 10.0f
            
        };

        pts.push_back(std::move(tempRoute));
    }

    TP_DN_SetRoutes(pts,10);

    TP_DN_StartRoutes(0);
    // TP_DN_StartRoutes(2);  //需要两个trigger信号才可运行

    return 0;
}



//---虚拟点位发送---
int DBG_Virtual_Pts_Set_Backword(tpDNPositionStruct & cur_pos)  //用到了 TP_DN_SetPosition接口
{
    int DBG_Num_Pts = 20000; //1000;
    std::vector<tpDNRouteStruct> pts;

    pts.reserve(DBG_Num_Pts);
    
    for (int i = 0; i < DBG_Num_Pts; ++i) {
        // 1. 创建临时对象（每次循环生成新对象）
        tpDNRouteStruct tempRoute{
            .pos = {
            .x = cur_pos.x  - (i+1) * (0.02 / 1.0f),
            .y = cur_pos.y  - (i+1) * (0.02 / 1.0f),
            .z = cur_pos.z  - (i+1) * (0.02 / 1.0f),
            .rx = cur_pos.rx  - (i+1) * (0.36 / 1.0f),
            .rz = cur_pos.rz  - (i+1) * (0.36  / 1.0f),
            .ry = cur_pos.ry  - 0, // 显式初始化保留字段
            },
            .vx = 200.0f,
            .vy = 200.0f,
            .vz = 200.0f,
            .vrx = 200.0f,
            .vry = 200.0f,  // 显式初始化保留字段
            .vrz = 200.0f,
            .ax = 10.0f,
            .ay = 10.0f,
            .az = 10.0f,
            .arx = 10.0f,
            .ary = 10.0f,   // 显式初始化保留字段
            .arz = 10.0f
            
        };

        pts.push_back(std::move(tempRoute));
    }

    if(TP_DN_SetRoutes(pts,10) == 0)
    {
        TP_DN_StartRoutes(0);
     // TP_DN_StartRoutes(2);  //需要两个trigger信号才可运行
    }
    else
    {
        std::cout<< "----------Send route potins failed !!! ------------- " <<std::endl;
    }

    
    return 0;
}


void TP_DN_SetCallBack(callback_trigger func)
{
    PtoIns.SetCallback(func);  
}

void TP_DN_SetCallBack_MotorErr(callback_motor_err func)
{
    PtoIns.SetCallback_Motor_Error(func);  
}